#ifdef STM32CUBE

#include <main.h>

#endif
#ifdef ARDUINO

#include <rtos.h>

#endif

#include <cstring>
#include <core_fault.h>
#include "QUAD_SPI.h"

/* Flag Status Register */
#define W25Q128FV_FSR_BUSY ((uint8_t)0x01) /*!< busy */

/**
 * @brief  This function send a Write Enable and wait it is effective.
 * @param  hqspi: QSPI handle
 * @retval None
 */
uint8_t QUAD_SPI::QSPI_WriteEnable() {
    QSPI_Send_CMD(WRITE_ENABLE, 0, (0 << 6) | (0 << 4) | (0 << 2) | (1 << 0), 0, 1);
    NORFLASH_Wait_Busy();
    int val = NORFLASH_ReadSR(0);
    if (0 == (val & 2)) {
        core_debug("\nWriteEnable Failed!\n");
    }
    return HAL_OK;
}

extern QSPI_HandleTypeDef hqspi;
extern "C" void MX_QUADSPI_Init(void);

int QUAD_SPI::init() {
    uint8_t pData[8];
    QSPI_CommandTypeDef s_command;
#ifndef STM32CUBE
    MX_QUADSPI_Init();
    // lock = rtos::Create_Mutex();
#endif
    exit_quad();
    // DisableMemoryMappedMode();
    /* QSPI memory reset */
    while (QSPI_ResetMemory() != HAL_OK) {
        MX_QUADSPI_Init();
    }
    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = READ_ID_CMD;
    s_command.AddressMode = QSPI_ADDRESS_1_LINE;
    s_command.AddressSize = QSPI_ADDRESS_24_BITS;
    s_command.Address = 0x000000;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DataMode = QSPI_DATA_1_LINE;
    s_command.DummyCycles = 0;
    s_command.NbData = 2;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    if (HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) {
        core_fault("QSPI ERR %s %d\n", __FILE__, __LINE__);
    }
    memset(pData, 0, 3);
    if (HAL_QSPI_Receive(&hqspi, pData, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) {
        core_fault("QSPI ERR %s %d\n", __FILE__, __LINE__);
    }
    core_debug("QSPI  I/0 Read Device ID : 0x%2X 0x%2X\r\n", pData[0], pData[1]);
    /* Read Manufacture/Device ID Dual I/O*/
    // s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    // s_command.Instruction = DUAL_READ_ID_CMD;
    // s_command.AddressMode = QSPI_ADDRESS_2_LINES;
    // s_command.AddressSize = QSPI_ADDRESS_24_BITS;
    // s_command.Address = 0x000000;
    // s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_2_LINES;
    // s_command.AlternateBytesSize = QSPI_ALTERNATE_BYTES_8_BITS;
    // s_command.AlternateBytes = 0;
    // s_command.DataMode = QSPI_DATA_2_LINES;
    // s_command.DummyCycles = 0;
    // s_command.NbData = 4;
    // s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    // s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    // s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    // if (HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    // {
    //     core_debug("QSPI ERR %s %d\n", __FILE__, __LINE__);
    //     Error_Handler();
    // }
    // memset(pData, 0, 3);
    // if (HAL_QSPI_Receive(&hqspi, pData, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    // {
    //     core_debug("QSPI ERR %s %d\n", __FILE__, __LINE__);
    //     Error_Handler();
    // }
    // core_debug("Dual I/O Read Device ID : 0x%2X 0x%2X\r\n", pData[0], pData[1]);
    /* Read Manufacture/Device ID Quad I/O*/
    // s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    // s_command.Instruction = QUAD_READ_ID_CMD;
    // s_command.AddressMode = QSPI_ADDRESS_4_LINES;
    // s_command.AddressSize = QSPI_ADDRESS_24_BITS;
    // s_command.Address = 0x000000;
    // s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_4_LINES;
    // s_command.AlternateBytesSize = QSPI_ALTERNATE_BYTES_8_BITS;
    // s_command.AlternateBytes = 0x00;
    // s_command.DataMode = QSPI_DATA_4_LINES;
    // s_command.DummyCycles = 4;
    // s_command.NbData = 2;
    // s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    // s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    // s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    // if (HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    // {
    //     core_debug("QSPI ERR %s %d\n", __FILE__, __LINE__);
    //     Error_Handler();
    // }
    // memset(pData, 0, 3);
    // if (HAL_QSPI_Receive(&hqspi, pData, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
    // {
    //     core_debug("QSPI ERR %s %d\n", __FILE__, __LINE__);
    //     Error_Handler();
    // }
    // core_debug("Quad I/O Read Device ID : 0x%2X 0x%2X\r\n", pData[0], pData[1]);
    /* Read JEDEC ID */
    QSPI_Send_CMD(JEDEC_ID, 0, (1 << 6) | (2 << 4) | (0 << 2) | (1 << 0), 0,
                  3); // QPI,快速读数据,地址为ReadAddr,4线传输数据_24位地址_4线传输地址_4线传输指令,8空周期,NumByteToRead个数据
    QSPI_Receive((uint8_t *) pData);
    _jedecID = (pData[0] << 16) | (pData[1] << 8) | pData[2];
    core_debug("Read JEDEC ID :  0x%X \r\n\r\n", _jedecID);
    // QSPI_Send_CMD(VOLATILE_SR_WRITE_ENABLE, 0, (0 << 6) | (0 << 4) | (0 << 2) | (1 << 0), 0);
    // NORFLASH_Write_SR(0, 0);
    pData[0] = NORFLASH_ReadSR(1);
    pData[1] = NORFLASH_ReadSR(2);
    pData[2] = NORFLASH_ReadSR(3);
    core_debug("status3 :  0x%2X 0x%2X 0x%2X\r\n\r\n", pData[0], pData[1], pData[2]);
    // enter_quad_mode();
    quad_mode = D1A1C1;
    return true;
}

int QUAD_SPI::writeBytes(uint32_t addr, const void *sbuf, uint16_t len) {
#ifndef STM32CUBE
    // rtos::mutex_lock(lock);
#endif
#ifdef QSPI_TEST
    uint8_t *buf;
    uint8_t *readbuf;
    buf = (uint8_t *)malloc(4096);
    readbuf = (uint8_t *)malloc(4096);
    memcpy(buf, sbuf, len);
    readBytes(addr, readbuf, len);
    for (int i = 0; i < len; i++)
    {
        if (readbuf[i] != 0xff)
        {
            core_debug("not empty!0x%x = 0x%x", addr + i, readbuf[i]);
        }
    }
#endif
    uint32_t count, saddr;
    const uint8_t *start;
    start = (const uint8_t *) sbuf;
    saddr = addr;
    do {
        if (((saddr & 0xff) + len) > 256) {
            count = (saddr | 0xff) + 1 - saddr;
        } else {
            count = len;
        }
        QSPI_WriteEnable(); // 写使能
        switch (quad_mode) {
            case D1A1C1:

            default:
                QSPI_Send_CMD(PAGE_PROGRAM, saddr, (1 << 6) | (2 << 4) | (1 << 2) | (1 << 0), 0,
                              count); // QPI,页写指令,地址为WriteAddr,4线传输数据_24位地址_4线传输地址_4线传输指令,无空周期,NumByteToWrite个数据
                break;
            case D4A4C1:
                QSPI_Send_CMD(QUAD_PAGE_PROGRAM, saddr, (3 << 6) | (2 << 4) | (1 << 2) | (1 << 0), 0,
                              count); // QPI,页写指令,地址为WriteAddr,4线传输数据_24位地址_4线传输地址_4线传输指令,无空周期,NumByteToWrite个数据
                break;
        }

        QSPI_Transmit((uint8_t *) start);
        NORFLASH_Wait_Busy(); // 等待写入结束
        saddr += count;
        start += count;
        len -= count;
    } while (len > 0);

#ifdef QSPI_TEST
    readBytes(addr, readbuf, len);
    if (memcmp(buf, readbuf, len) != 0)
    {
        core_debug("QSPI Write error!!");
    }
    free(readbuf);
    free(buf);
#endif
#ifndef STM32CUBE
    // rtos::mutex_unlock(lock);
#endif
    return HAL_OK;
}

int QUAD_SPI::writeRamBytes(uint32_t addr, const void *sbuf, uint16_t len) {
#ifndef STM32CUBE
    // rtos::mutex_lock(lock);
#endif
    QSPI_Send_CMD(0x38, addr, (3 << 6) | (2 << 4) | (3 << 2) | (3 << 0),
                  0, len); // QPI,页写指令,地址为WriteAddr,4线传输数据_24位地址_4线传输地址_4线传输指令,无空周期,NumByteToWrite个数据
    QSPI_Transmit((uint8_t *) sbuf);
    // NORFLASH_Wait_Busy(); //等待写入结束
#ifndef STM32CUBE
    // rtos::mutex_unlock(lock);
#endif
    return HAL_OK;
}

int QUAD_SPI::readBytes(uint32_t address, void *buf, uint16_t count) {
#ifndef STM32CUBE
    // rtos::mutex_lock(lock);
#endif
    NORFLASH_Wait_Busy();
    switch (quad_mode) {
        case D1A1C1:

        default:
            QSPI_Send_CMD(FAST_READ_DATA, address, (1 << 6) | (2 << 4) | (1 << 2) | (1 << 0), 8,
                          count); // QPI,快速读数据,地址为ReadAddr,4线传输数据_24位地址_4线传输地址_4线传输指令,8空周期,NumByteToRead个数据
            break;
        case D4A4C1:
            QSPI_Send_CMD(QUAD_INOUT_FAST_READ_CMD, address, (3 << 6) | (2 << 4) | (3 << 2) | (1 << 0), 4,
                          count); // QPI,快速读数据,地址为ReadAddr,4线传输数据_24位地址_4线传输地址_4线传输指令,8空周期,NumByteToRead个数据
            break;
    }
    QSPI_Receive((uint8_t *) buf);
#ifndef STM32CUBE
    // rtos::mutex_unlock(lock);
#endif
    return HAL_OK;
}

int QUAD_SPI::readRamBytes(uint32_t address, void *buf, uint16_t count) {
    // NORFLASH_Wait_Busy();
#ifndef STM32CUBE
    // rtos::mutex_lock(lock);
#endif
    QSPI_Send_CMD(QUAD_INOUT_FAST_READ_CMD, address, (3 << 6) | (2 << 4) | (3 << 2) | (3 << 0), 6,
                  count); // QPI,快速读数据,地址为ReadAddr,4线传输数据_24位地址_4线传输地址_4线传输指令,8空周期,NumByteToRead个数据
    QSPI_Receive((uint8_t *) buf);
#ifndef STM32CUBE
    // rtos::mutex_unlock(lock);
#endif
    return HAL_OK;
}

int QUAD_SPI::exit_quad() {
    QSPI_CommandTypeDef s_command;

    /* Configure the EXIT QPI Mode command */
    s_command.InstructionMode = QSPI_INSTRUCTION_4_LINES;
    s_command.Instruction = 0xff;
    s_command.AddressMode = QSPI_ADDRESS_NONE;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DataMode = QSPI_DATA_NONE;
    s_command.DummyCycles = 0;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    /* Send the command */
    if (HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) {
        core_debug("QSPI ERR %s %d\n", __FILE__, __LINE__);
        return HAL_ERROR;
    }
    quad_mode = D1A1C1;
    return HAL_OK;
}

int QUAD_SPI::enter_quad_mode(uint8_t _quad_mode) {

    uint8_t stareg2;
    stareg2 = NORFLASH_ReadSR(2); // 先读出状态寄存器2的原始值
    core_debug("stareg2:%x\r\n", stareg2);
    if ((stareg2 & 0X02) == 0) // QE位未使能
    {
        QSPI_WriteEnable();            // 写使能
        stareg2 |= 1 << 1;             // 使能QE位
        NORFLASH_Write_SR(2, stareg2); // 写状态寄存器2
    }
    QSPI_Send_CMD(ENTER_QUAD_MODE, 0, (0 << 6) | (0 << 4) | (0 << 2) | (1 << 0),
                  0, 1); // 写command指令,地址为0,无数据_8位地址_无地址_单线传输指令,无空周期,0个字节数据
    quad_mode = _quad_mode;
    return HAL_OK;
}

int QUAD_SPI::enter_ram_quad_mode() {

    QSPI_Send_CMD(0x35, 0, (0 << 6) | (0 << 4) | (0 << 2) | (1 << 0),
                  0, 1); // 写command指令,地址为0,无数据_8位地址_无地址_单线传输指令,无空周期,0个字节数据

    return HAL_OK;
}

int QUAD_SPI::blockErase4K(uint32_t address) {
#ifndef STM32CUBE
    // rtos::mutex_lock(lock);
#endif
    QSPI_WriteEnable(); // SET WEL
    NORFLASH_Wait_Busy();
    QSPI_Send_CMD(SECTOR_ERASE, address, (0 << 6) | (2 << 4) | (1 << 2) | (1 << 0),
                  0, 1);  // QPI,写扇区擦除指令,地址为0,无数据_24位地址_4线传输地址_4线传输指令,无空周期,0个字节数据
    NORFLASH_Wait_Busy(); // 等待擦除完成
#ifdef QSPI_TEST
    uint8_t *readbuf;
    readbuf = (uint8_t *)malloc(4096);
    assert(readbuf);
    memset(readbuf, 0, 4096);
    readBytes(address, readbuf, 4096);
    for (int i = 0; i < 4096; i++)
    {
        if (readbuf[i] != 0xff)
        {
            core_debug("Erase Failed. 0x%x = 0x%x", address + i, readbuf[i]);
        }
    }
    free(readbuf);
#endif
#ifndef STM32CUBE
    // rtos::mutex_unlock(lock);
#endif
    return HAL_OK;
}

uint8_t QUAD_SPI::QSPI_AutoPollingMemReady(uint32_t Timeout) {
    QSPI_CommandTypeDef s_command;
    QSPI_AutoPollingTypeDef s_config;

    /* Configure automatic polling mode to wait for memory ready */
    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = READ_STATUS_REGISTER;
    s_command.AddressMode = QSPI_ADDRESS_NONE;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DataMode = QSPI_DATA_1_LINE;
    s_command.DummyCycles = 0;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    s_config.Match = 0x00;
    s_config.Mask = W25Q128FV_FSR_BUSY;
    s_config.MatchMode = QSPI_MATCH_MODE_AND;
    s_config.StatusBytesSize = 1;
    s_config.Interval = 0x10;
    s_config.AutomaticStop = QSPI_AUTOMATIC_STOP_ENABLE;

    if (HAL_QSPI_AutoPolling(&hqspi, &s_command, &s_config, Timeout) != HAL_OK) {
        return HAL_ERROR;
    }

    return HAL_OK;
}

uint8_t QUAD_SPI::QSPI_ResetMemory() {
    QSPI_CommandTypeDef s_command;

    /* Initialize the reset enable command */
    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = RESET_ENABLE;
    s_command.AddressMode = QSPI_ADDRESS_NONE;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DataMode = QSPI_DATA_NONE;
    s_command.DummyCycles = 0;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    /* Send the command */
    if (HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) {
        core_debug("QSPI ERR %s %d\n", __FILE__, __LINE__);
        return HAL_ERROR;
    }

    /* Send the reset memory command */
    s_command.Instruction = RESET_DEVICE;
    if (HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) {
        core_debug("QSPI ERR %s %d\n", __FILE__, __LINE__);
        return HAL_ERROR;
    }

    /* Configure automatic polling mode to wait the memory is ready */
    if (QSPI_AutoPollingMemReady(HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) {
        core_debug("QSPI ERR %s %d\n", __FILE__, __LINE__);
        return HAL_ERROR;
    }

    return HAL_OK;
}

// QSPI发送命令
// cmd:要发送的指令
// addr:发送到的目的地址
// mode:模式,详细位定义如下:
//	mode[1:0]:指令模式;00,无指令;01,单线传输指令;10,双线传输指令;11,四线传输指令.
//	mode[3:2]:地址模式;00,无地址;01,单线传输地址;10,双线传输地址;11,四线传输地址.
//	mode[5:4]:地址长度;00,8位地址;01,16位地址;10,24位地址;11,32位地址.
//	mode[7:6]:数据模式;00,无数据;01,单线传输数据;10,双线传输数据;11,四线传输数据.
// dmcycle:空指令周期数
void QUAD_SPI::QSPI_Send_CMD(uint8_t cmd, uint32_t addr, uint8_t mode, uint8_t dmcycle, uint32_t dataSize) {
    QSPI_CommandTypeDef Cmdhandler;

    Cmdhandler.Instruction = cmd;     // 指令
    Cmdhandler.Address = addr;        // 地址
    Cmdhandler.DummyCycles = dmcycle; // 设置空指令周期数

    if (((mode >> 0) & 0x03) == 0)
        Cmdhandler.InstructionMode = QSPI_INSTRUCTION_NONE; // 指令模式
    else if (((mode >> 0) & 0x03) == 1)
        Cmdhandler.InstructionMode = QSPI_INSTRUCTION_1_LINE; // 指令模式
    else if (((mode >> 0) & 0x03) == 2)
        Cmdhandler.InstructionMode = QSPI_INSTRUCTION_2_LINES; // 指令模式
    else if (((mode >> 0) & 0x03) == 3)
        Cmdhandler.InstructionMode = QSPI_INSTRUCTION_4_LINES; // 指令模式

    if (((mode >> 2) & 0x03) == 0)
        Cmdhandler.AddressMode = QSPI_ADDRESS_NONE; // 地址模式
    else if (((mode >> 2) & 0x03) == 1)
        Cmdhandler.AddressMode = QSPI_ADDRESS_1_LINE; // 地址模式
    else if (((mode >> 2) & 0x03) == 2)
        Cmdhandler.AddressMode = QSPI_ADDRESS_2_LINES; // 地址模式
    else if (((mode >> 2) & 0x03) == 3)
        Cmdhandler.AddressMode = QSPI_ADDRESS_4_LINES; // 地址模式

    if (((mode >> 4) & 0x03) == 0)
        Cmdhandler.AddressSize = QSPI_ADDRESS_8_BITS; // 地址长度
    else if (((mode >> 4) & 0x03) == 1)
        Cmdhandler.AddressSize = QSPI_ADDRESS_16_BITS; // 地址长度
    else if (((mode >> 4) & 0x03) == 2)
        Cmdhandler.AddressSize = QSPI_ADDRESS_24_BITS; // 地址长度
    else if (((mode >> 4) & 0x03) == 3)
        Cmdhandler.AddressSize = QSPI_ADDRESS_32_BITS; // 地址长度

    if (((mode >> 6) & 0x03) == 0)
        Cmdhandler.DataMode = QSPI_DATA_NONE; // 数据模式
    else if (((mode >> 6) & 0x03) == 1)
        Cmdhandler.DataMode = QSPI_DATA_1_LINE; // 数据模式
    else if (((mode >> 6) & 0x03) == 2)
        Cmdhandler.DataMode = QSPI_DATA_2_LINES; // 数据模式
    else if (((mode >> 6) & 0x03) == 3)
        Cmdhandler.DataMode = QSPI_DATA_4_LINES; // 数据模式

    Cmdhandler.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;           // 每次都发送指令
    Cmdhandler.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; // 无交替字节
    Cmdhandler.DdrMode = QSPI_DDR_MODE_DISABLE;               // 关闭DDR模式
    Cmdhandler.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    Cmdhandler.NbData = dataSize;
    // Cmdhandler.AlternateBytesSize = QSPI_ALTERNATE_BYTES_24_BITS;
    // Cmdhandler.AlternateBytes = addr;

    if (HAL_QSPI_Command(&hqspi, &Cmdhandler, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) {
        core_debug("QSPI ERR %s %d\n", __FILE__, __LINE__);
    }
}

// QSPI接收指定长度的数据
// buf:接收数据缓冲区首地址
// datalen:要传输的数据长度
// 返回值:0,正常
//     其他,错误代码
uint8_t QUAD_SPI::QSPI_Receive(uint8_t *buf) {
    //    hqspi.Instance->DLR = datalen - 1; //配置数据长度
    if (HAL_QSPI_Receive(&hqspi, buf, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) == HAL_OK)
        return 0; // 接收数据
    else {
        core_debug("QSPI_Receive Error");
        return 1;
    }
}

// QSPI发送指定长度的数据
// buf:发送数据缓冲区首地址
// datalen:要传输的数据长度
// 返回值:0,正常
//     其他,错误代码
uint8_t QUAD_SPI::QSPI_Transmit(uint8_t *buf) {
    //    hqspi.Instance->DLR = datalen - 1; //配置数据长度
    if (HAL_QSPI_Transmit(&hqspi, buf, HAL_QPSI_TIMEOUT_DEFAULT_VALUE) == HAL_OK)
        return 0; // 发送数据
    else {
        core_debug("QSPI_Transmit Error.");
        return 1;
    }
}

// 读取W25QXX的状态寄存器，W25QXX一共有3个状态寄存器
// 状态寄存器1：
//  BIT7  6   5   4   3   2   1   0
//  SPR   RV  TB BP2 BP1 BP0 WEL BUSY
//  SPR:默认0,状态寄存器保护位,配合WP使用
//  TB,BP2,BP1,BP0:FLASH区域写保护设置
//  WEL:写使能锁定
//  BUSY:忙标记位(1,忙;0,空闲)
// 默认:0x00
// 状态寄存器2：
//  BIT7  6   5   4   3   2   1   0
//  SUS   CMP LB3 LB2 LB1 (R) QE  SRP1
// 状态寄存器3：
//  BIT7      6    5    4   3   2   1   0
//  HOLD/RST  DRV1 DRV0 (R) (R) WPS ADP ADS
//  regno:状态寄存器号，范:1~3
// 返回值:状态寄存器值
uint8_t QUAD_SPI::NORFLASH_ReadSR(uint8_t regno) {
    uint8_t byte = 0, command;
    switch (regno) {
        case 1:
            command = READ_STATUS_REGISTER; // 读状态寄存器1指令
            break;
        case 2:
            command = READ_STATUS_REGISTER_2; // 读状态寄存器2指令
            break;
        case 3:
            command = READ_STATUS_REGISTER_3; // 读状态寄存器3指令
            break;
        default:
            command = READ_STATUS_REGISTER;
            break;
    }
    QSPI_Send_CMD(command, 0, (1 << 6) | (0 << 4) | (0 << 2) | (1 << 0),
                  0, 1); // QPI,写command指令,地址为0,4线传数据_8位地址_无地址_4线传输指令,无空周期,1个字节数据
    QSPI_Receive(&byte);
    return byte;
}

// 写W25QXX状态寄存器
void QUAD_SPI::NORFLASH_Write_SR(uint8_t regno, uint8_t sr) {
    uint8_t command;
    switch (regno) {
        case 1:
            command = WRITE_STATUS_REGISTER; // 写状态寄存器1指令
            break;
        case 2:
            command = WRITE_STATUS_REGISTER_2; // 写状态寄存器2指令
            break;
        case 3:
            command = WRITE_STATUS_REGISTER_3; // 写状态寄存器3指令
            break;
        default:
            command = WRITE_STATUS_REGISTER;
            break;
    }
    QSPI_Send_CMD(command, 0, (1 << 6) | (0 << 4) | (0 << 2) | (1 << 0),
                  0, 1); // QPI,写command指令,地址为0,4线传数据_8位地址_无地址_4线传输指令,无空周期,1个字节数据
    QSPI_Transmit(&sr);
}

void QUAD_SPI::NORFLASH_Wait_Busy() {
    uint32_t t = HAL_GetTick() + 500;
    while ((NORFLASH_ReadSR(1) & 0x01) == 0x01) {
        if (HAL_GetTick() > t) {
            core_debug("NORFLASH_Wait_Busy timeout");
            break;
        }
    } // 等待BUSY位清空
}

int QUAD_SPI::EnableMemoryMappedMode() {
    QSPI_CommandTypeDef s_command;
    QSPI_MemoryMappedTypeDef s_mem_mapped_cfg;

    /* Configure the command for the read instruction */
    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = QUAD_INOUT_FAST_READ_CMD;
    s_command.AddressMode = QSPI_ADDRESS_4_LINES;
    s_command.AddressSize = QSPI_ADDRESS_24_BITS;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DataMode = QSPI_DATA_4_LINES;
    s_command.DummyCycles = 6;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_HALF_CLK_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    /* Configure the memory mapped mode */
    s_mem_mapped_cfg.TimeOutActivation = QSPI_TIMEOUT_COUNTER_DISABLE;
    s_mem_mapped_cfg.TimeOutPeriod = 0;

    return HAL_QSPI_MemoryMapped(&hqspi, &s_command, &s_mem_mapped_cfg);
}

int QUAD_SPI::DisableMemoryMappedMode() {
    HAL_QSPI_Abort(&hqspi);
    return 0;
}

void QUAD_SPI::chipErase() {
#ifndef STM32CUBE
    // rtos::mutex_lock(lock);
#endif
    QSPI_WriteEnable(); // SET WEL
    NORFLASH_Wait_Busy();
    QSPI_Send_CMD(CHIP_ERASE, 0, (0 << 6) | (0 << 4) | (0 << 2) | (1 << 0), 0,
                  1);     // QPI,写扇区擦除指令,地址为0,无数据_24位地址_4线传输地址_4线传输指令,无空周期,0个字节数据
    NORFLASH_Wait_Busy(); // 等待擦除完成
#ifdef QSPI_TEST
    uint8_t *readbuf;
    readbuf = (uint8_t *)malloc(4096);
    assert(readbuf);
    memset(readbuf, 0, 4096);
    readBytes(address, readbuf, 4096);
    for (int i = 0; i < 4096; i++)
    {
        if (readbuf[i] != 0xff)
        {
            core_debug("Erase Failed. 0x%x = 0x%x", address + i, readbuf[i]);
        }
    }
    free(readbuf);
#endif
#ifndef STM32CUBE
    // rtos::mutex_unlock(lock);
#endif
}

int QUAD_SPI::erasePage(uint32_t address, uint32_t sz) {
    for (int i = 0; i < sz; i += 4096) {
        blockErase4K(address + i);
    }
    return 0;
}
